#ifndef SparseVector_h__
#define SparseVector_h__

#include <cassert>

namespace UGSIM {

template<class TYPE>
class sp_vec {
public:
    sp_vec() {
        key_size = 0;
        pool_size = 0;
        next_in_pool = 0;
        keys = nullptr;
        pool = nullptr;
    };

    sp_vec(const size_t size) {
        key_size = size;
        keys = new size_t[size];
        for (size_t i = 0; i < size; ++i) { keys[i] = (size_t)-1; }

        pool_size = __max(__min(size, 16), 1);
        pool = new TYPE[pool_size];
        next_in_pool = 0;
    };

    sp_vec(const sp_vec& src) {
        key_size = src.key_size;
        keys = new size_t[src.key_size];
        memcpy(keys, src.keys, src.key_size * sizeof(size_t));

        pool_size = src.pool_size;
        pool = new TYPE[src.pool_size];
        for (size_t i = 0; i < pool_size; ++i) {
            pool[i] = src.pool[i];
        }
        next_in_pool = src.next_in_pool;
    };

    void operator=(const sp_vec& src) {
        key_size = src.key_size;
        delete[] keys; keys = new size_t[src.key_size];
        memcpy(keys, src.keys, src.key_size * sizeof(size_t));

        pool_size = src.pool_size;
        delete[] pool; pool = new TYPE[src.pool_size];
        for (size_t i = 0; i < pool_size; ++i) {
            pool[i] = src.pool[i];
        }
        next_in_pool = src.next_in_pool;
    };

    ~sp_vec() {
        delete[] keys;
        delete[] pool;
    };

    TYPE& operator[](const size_t i)
    {
        assert(i < key_size);

        if (keys[i] == -1) {
            if (next_in_pool >= pool_size)
            {
                extend_pool();
            }

            keys[i] = next_in_pool;
            ++next_in_pool;
        }

        return pool[keys[i]];
    };

    const TYPE& operator[](const size_t i) const
    {
        assert(i < key_size && keys[i] >= 0);
        return pool[keys[i]];
    };

    bool is_empty() const {
        return (next_in_pool == 0);
    };

    bool is_filled(const size_t i) const {
        return (i < key_size && keys[i] >= 0);
    };

private:
    void extend_pool() {
        size_t min_extend = 16;
        size_t max_extend = 1024 * 256 / sizeof(TYPE);

        size_t new_pool_size = 2 * pool_size;

        new_pool_size = __max(new_pool_size, pool_size + min_extend);
        new_pool_size = __min(new_pool_size, pool_size + max_extend);
        new_pool_size = __min(new_pool_size, key_size);

        TYPE* new_pool = new TYPE[new_pool_size];

        for (size_t i = 0; i < pool_size; ++i) { new_pool[i] = pool[i]; }

        delete[] pool;

        pool = new_pool;
        pool_size = new_pool_size;
    };

    size_t key_size;
    size_t pool_size;
    size_t next_in_pool;
    size_t* keys;
    TYPE* pool;
};

}

#endif // SparseVector_h__